#ifndef PlanePointRansacMatcherI_H_
#define PlanePointRansacMatcherI_H_
//OpenCV
#include "cv.h"
#include "highgui.h"
#include <string>
#include <iostream>
#include <stdio.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/common/transformation_from_correspondences.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/thread/thread.hpp>

//g2o
#include "g2o/core/graph_optimizer_sparse.h"
#include "g2o/core/hyper_graph.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/math_groups/se3quat.h"
#include "g2o/core/structure_only_solver.h"
#include "g2o/types/slam3d/vertex_se3_quat.h"
#include "g2o/types/slam3d/edge_se3_quat.h"

#include "AssociationSet.h"

using namespace std;

class FrameNode;
class Transformation;
class Map;

typedef std::vector<FrameNode, Eigen::aligned_allocator<Eigen::Vector4f> > FrameNodeVector;

class PlanePointRansacMatcherI: public FrameMatcher
{
	public:
		g2o::VertexSE3 * vertexSE3_src;
		g2o::VertexSE3 * vertexSE3_dst;
		//std::vector<g2o::BaseVertex * > vertexes;
		PlanePointRansacMatcherI();
		~PlanePointRansacMatcherI();
		Transformation * getTransformation(FrameNode * src, FrameNode * dst);
		void update();
		void associate(FrameNode * src, FrameNode * dst);
	private:
	g2o::SparseOptimizer graphoptimizer;
};

#endif
